#! /usr/bin/env python

PACKAGE='ptam'
import roslib; roslib.load_manifest(PACKAGE)

#from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator import *

gen = ParameterGenerator()

# @todo Double check these ranges, defaults

enum_bundle = gen.enum([
gen.const("LOCAL_GLOBAL",   str_t, "LOCAL_GLOBAL",  "local and global bundle adjustment"),
gen.const("LOCAL",          str_t, "LOCAL",         "local bundle adjustment only"),
gen.const("GLOBAL",         str_t, "GLOBAL",        "global bundle adjustment only")
], "An enum to set the bundle adjustment mode.")

enum_FAST = gen.enum([
gen.const("FAST9",          str_t, "FAST9",         "FAST 9"),
gen.const("FAST10",         str_t, "FAST10",        "FAST 10"),
gen.const("FAST9_nonmax",   str_t, "FAST9_nonmax",  "FAST 9 with nonmax suppression"),
gen.const("AGAST12d",       str_t, "AGAST12d",      "AGAST 12 pixel diamond"),
gen.const("OAST16",       str_t, "OAST16",          "AGAST 16 pixel circular")
], "An enum to set the FAST corner extraction method.")

enum_MOTIONMODEL = gen.enum([
gen.const("MM_CONSTANT",       str_t, "CONSTANT",      "use constant motion model."),
gen.const("MM_IMU",            str_t, "IMU",           "use imu orientation for the motion model."),
gen.const("MM_FULL_POSE",      str_t, "FULL_POSE",     "use full pose estimated externally for motion model.")
], "An enum to set wich input to use for the motion model.")

RESTART_AR     = gen.const("SEND_PARAMETERS",          int_t, 0x00000001, "send_parameters")


#       Name                         Type      Reconfiguration lvl    Description                                                   Default             Min     Max
gen.add("Scale",                            double_t,   0,              "Scale",                                                        1,              0.01,   30)
gen.add("MotionModelSource",                str_t,      0,              "selects the source for the motion model.",                     "CONSTANT", edit_method=enum_MOTIONMODEL) 
gen.add("MaxPatchesPerFrame",               double_t,   0,              "max features per frame",                                       500,            10,     1000)
gen.add("MaxKFDistWiggleMult",              double_t,   0,              "'distance' after which a new kf is requested",                  3,              0.1,    10)
gen.add("UseKFPixelDist",                   bool_t,     1,              "use AutoInitPixel as new KF request criteria",               True)
gen.add("NoLevelZeroMapPoints",             bool_t,     1,              "do not add map points at level zero",                        True)
gen.add("EpiDepthSearchMaxRange",           double_t,   0,              "depth variance to search for features",                        100,            1,      100)
gen.add("CoarseMin",                        double_t,   0,              "min number of features for coarse tracking",                   20,             1,      100)
gen.add("CoarseMax",                        double_t,   0,              "max number of features for coarse tracking",                   60,             1,      100)
gen.add("CoarseRange",                      double_t,   0,              "Pixel search radius for coarse features",                      30,             1,      100)
gen.add("CoarseSubPixIts",                  double_t,   0,              "coarse tracking sub-pixel iterations",                         8,              1,      100)
gen.add("DisableCoarse",                    bool_t,     0,              "enable/disable coarse tracking",                               False)
gen.add("CoarseMinVelocity",                double_t,   0,              "speed above which coarse stage is used",                       0.006,          0,      1)
gen.add("TrackingQualityGood",              double_t,   0,              "min ratio features visible/features found for good tracking",  0.3,            0,      1)
gen.add("TrackingQualityLost",              double_t,   0,              "max ratio features visible/features found before lost",        0.13,           0,      1)
gen.add("TrackingQualityFoundPixels",       int_t,      0,              "min pixels to be found for good tracking",                     100,             1,      1000)
gen.add("MaxIterations",                    double_t,   0,              "max iterations for bundle adjustment",                         20,             1,      100)
gen.add("MaxKF",                            int_t,      0,              "number of keyframes kept in the map (0 or 1 = inf)",           0,              0,      1000)
gen.add("BundleMethod",                     str_t,      0,              "bundleadjustment method",                                      "LOCAL_GLOBAL", edit_method=enum_bundle)

gen.add("UpdateSquaredConvergenceLimit",    double_t,   0,              "limit for convergence in bundle adjustment",                   0.000001,       0,      1)
gen.add("BundleDebugMessages",              bool_t,     0,              "print bundle debug messages",                                  False)
gen.add("FASTMethod",                       str_t,      0,              "FAST corner method",                                           "OAST16", edit_method=enum_FAST)
gen.add("Thres_lvl0",                       int_t,      0,              "threshold for FAST features on level 0",                       0,             0,      255)
gen.add("Thres_lvl1",                       int_t,      0,              "threshold for FAST features on level 1",                       0,             0,      255)
gen.add("Thres_lvl2",                       int_t,      0,              "threshold for FAST features on level 2",                       0,             0,      255)
gen.add("Thres_lvl3",                       int_t,      0,              "threshold for FAST features on level 3",                       0,             0,      255)
gen.add("AdaptiveThrs",                     bool_t,     0,              "adaptive threshold for corner extraction",                     False)
gen.add("AdaptiveThrsMult",                 double_t,   0,              "controls adaptive threshold to MaxPatches*N corners",        5,		0.5,	20)

gen.add("RotationEstimatorBlur",            double_t,   0,              "small images for the rotation estimator blur",                 0.75,           0,      10)
gen.add("UseRotationEstimator",             bool_t,     0,              "small images for the rotation estimator enable/disable",       True)
gen.add("MiniPatchMaxSSD",                  int_t,      0,              "MiniPatch tracking threshhold",                                100000,         0,      10000000)

gen.add("PlaneAlignerRansacs",              int_t,      0,              "number of dominant plane RANSACs",                             100,            0,      1000)
gen.add("RelocMaxScore",                    int_t,      0,              "score for relocalization",                                     9000000,        0,      90000000)

gen.add("AutoInit",                    	    bool_t,     0,              "enable auto initialization",                                  False)
gen.add("AutoInitPixel",                    int_t,      0,              "min pixel distance for auto initialization",                   20,     1,  100)
gen.add("MaxStereoInitLoops",               int_t,      0,              "max # of loops for stereo initialization",                     10,     1,  100)

exit(gen.generate(PACKAGE, "Config", "PtamParams"))
